#include "led.h"

static constexpr rt_base_t ARM_LEDR = GET_PIN(B, 2);
static constexpr rt_base_t ARM_LEDG = GET_PIN(A, 10);
static constexpr rt_base_t ARM_LEDB = GET_PIN(A, 9);

void LED::run(void *parameter)
{
    rt_pin_mode(ARM_LEDR, PIN_MODE_OUTPUT);
    rt_pin_mode(ARM_LEDG, PIN_MODE_OUTPUT);
    rt_pin_mode(ARM_LEDB, PIN_MODE_OUTPUT);

    this->setLed(ARM_LEDR, LedState::OFF);
    this->setLed(ARM_LEDG, LedState::OFF);
    this->setLed(ARM_LEDB, LedState::OFF);

    bool ledRun = false;
    while (1)
    {
        if (this->ledSem->wait(0))
        {
            ledRun = !ledRun;
        }

        if (ledRun)
            this->ledLoop();
        else
        {
            this->setLed(ARM_LEDR, LedState::OFF);
            this->setLed(ARM_LEDG, LedState::OFF);
            this->setLed(ARM_LEDB, LedState::OFF);
            rt_thread_mdelay(500);
        }
    }
}

void LED::setLed(rt_base_t led, LedState state)
{
    if (state == LedState::ON)
    {
        rt_pin_write(led, PIN_LOW);
    }
    else
    {
        rt_pin_write(led, PIN_HIGH);
    }
}

void LED::ledLoop()
{
    this->setLed(ARM_LEDR, LedState::ON);
    this->setLed(ARM_LEDG, LedState::OFF);
    this->setLed(ARM_LEDB, LedState::OFF);
    rt_thread_mdelay(500);
    this->setLed(ARM_LEDR, LedState::OFF);
    this->setLed(ARM_LEDG, LedState::ON);
    this->setLed(ARM_LEDB, LedState::OFF);
    rt_thread_mdelay(500);
    this->setLed(ARM_LEDR, LedState::OFF);
    this->setLed(ARM_LEDG, LedState::OFF);
    this->setLed(ARM_LEDB, LedState::ON);
    rt_thread_mdelay(500);
}
